#include "bsp_mpu6050.h"
#include "bsp_i2c.h"
#include <stdlib.h>
#include "Delay.h"
#include "bsp_led.h"
#include "bsp_usart.h"

/* 灵敏度系数 */
#define Acc_LSBSens     (16384.0f)              // 加速度计灵敏度：满量程为+-2g时，1g==16384；为+-4g时16384/2
#define Gyro_LSBSens    (16.4f)                 // 陀螺仪灵敏度：满量程为+-2000 deg/s时，16.4

/* 单位转换系数 */
#define G               (9.80665f)              // 重力加速度g，单位：m/s^2
#define Deg_To_Rad      (0.01745329f)           // pi/180
#define Acc_Gain        (0.000598550415)        // 乘以1.0f/Acc_LSBSens将单位转换为重力加速度g，再乘G转换为m/s2
#define Gyro_Gain_Deg_S (0.060975609756)        // 1.0f/Gyro_LSBSens, 单位转换为°/s
#define Gyro_Gain_Rad_S (0.001064224999)        // (Gyro_Gain_Deg_S) * (Deg_To_Rad)°/s转换成rad/s

/* 用于校准传感器 */
static int16_t MpuGyroOffset[3] = {0};  // 陀螺仪绝对静止下的偏移
static int16_t MpuAccOffset[3] = {0};   // 加速度计初始偏差

/**
 * @brief 初始化MPU6050 检测ID是否正确，此处不初始化i2C
 *
 * @return MPU6050_t* 
 */
MPU6050_t* MPU6050_Init()
{
    uint8_t ID = 0;

    MPU6050_t *MPU6050 = (MPU6050_t *)malloc(sizeof(MPU6050_t));
    if (MPU6050 == NULL) {
        goto malloc_failed;
    }

    /* 初始化I2C */
    // I2C_Config();

    /* 配置MPU6050的参数 */
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_PWR_MGMT1_REG, 0X80);   // 复位MPU6050
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_PWR_MGMT1_REG, 0X00);   // 唤醒MPU6050
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_INT_EN_REG, 0X00);      // 关闭所有中断
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_USER_CTRL_REG, 0X00);   // I2C主模式关闭
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_FIFO_EN_REG, 0X00);     // 关闭FIFO
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_INTBP_CFG_REG, 0X80);   // INT引脚低电平有效
    MPU_Set_Gyro_Fsr(3);        // 陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);       // 加速度传感器,±2g
    MPU_Set_Rate(1000);           // 设置采样率50Hz

    /* 获取并检验ID号 */
    ID = MPU_GetID();
    MPU6050->devID = ID;
    if (ID == MPU_ID) {
        LED_G_ON;
        Delay_ms(500);
        LED_G_OFF;
    }
    // printf("\nMPU6050:0x%2x\n", ID);

    /* 启动MPU6050 */
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_PWR_MGMT1_REG, 0X01);// 设置CLKSEL,PLL X轴为参考
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_PWR_MGMT2_REG, 0X00);// 启动加速度与陀螺仪

    /* 校准传感器 */
    MpuGetOffset(500, 2);
    // printf("MPU6050 校准完成！\r\n");

    return MPU6050;

malloc_failed:
    if (MPU6050 != NULL) free(MPU6050);
    MPU6050 = NULL;
    return NULL;
}

/**
 * @brief 复位MPU6050，释放内存
 * 
 * @param pMPU 指向MPU6050_t类型的指针
 * @return uint8_t  1 -- 释放内存成功
 *                  0 -- 无内存可释放
 */
uint8_t MPU6050_DeInit(MPU6050_t *pMPU)
{
    if (pMPU != NULL) {
        free(pMPU);
        pMPU = NULL;
        return 1;
    }
    return 0;
}

/**
 * @brief 设置MPU6050陀螺仪传感器满量程范围,
 *
 * @param fsr 0 -> ±250dps;
 *            1 -> ±500dps;
 *            2 -> ±1000dps;
 *            3 -> ±2000dps
 * @return void 
 */
void MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_GYRO_CFG_REG, fsr << 3);
}

/**
 * @brief 设置MPU6050加速度传感器满量程范围
 *
 * @param fsr 0 -> ±2g;
 *            1 -> ±4g;
 *            2 -> ±8g;
 *            3 -> ±16g
 * g = 9.8m/s2
 * @return void 
 */
void MPU_Set_Accel_Fsr(uint8_t fsr)
{
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_ACCEL_CFG_REG, fsr << 3);
}

/**
 * @brief 设置MPU6050的数字低通滤波器
 *
 * @param lpf 数字低通滤波频率(Hz)
 * @return void 
 */
void MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data = 0;
    if (lpf >= 188)     data = 1;
    else if (lpf >= 98) data = 2;
    else if (lpf >= 42) data = 3;
    else if (lpf >= 20) data = 4;
    else if (lpf >= 10) data = 5;
    else                data = 6;
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_CFG_REG, data);
}

/**
 * @brief 设置MPU6050的采样率(假定Fs=1KHz)
 *
 * @param rate 4~1000(Hz)
 * @return void 
 */
void MPU_Set_Rate(uint16_t rate)
{
    uint8_t data = 0;
    if (rate > 1000) rate = 1000;
    if (rate < 4)    rate = 4;
    data = 1000 / rate - 1;
    I2Cx_SendByte(MPU6050_I2C, MPU_ADDR, MPU_SAMPLE_RATE_REG, data);
    MPU_Set_LPF(rate / 2);
}

/**
 * @brief 获取设备ID号
 *
 * @return uint8_t  器件ID号
 */
uint8_t MPU_GetID()
{
    return I2Cx_ReadByte(MPU6050_I2C, MPU_ADDR, MPU_DEVICE_ID_REG);
}

/**
 * @brief 获取温度值
 *
 * @return float_t 温度值（摄氏度）
 */
float_t MPU_GetTemperature(void)
{
    uint8_t buf[2] = {0};
    int16_t raw = 0;
    float_t temp = 0;

    I2Cx_ReadBuff(MPU6050_I2C, MPU_ADDR, MPU_TEMP_OUTH_REG, buf, 2);

    raw = ((int16_t)buf[0] << 8) | buf[1];        // 合并高位和低位
    temp = ((float_t)raw / 340.0f + 36.53f ) * 100;  // 转换为摄氏度

    return temp / 100.0f;
}

/**
 * @brief 获取陀螺仪的角速度值(原始值)
 *
 * @param gx x轴角速度值
 * @param gy y轴角速度值
 * @param gz z轴角速度值
 * @return uint8_t 
 */
uint8_t MPU_GetGyro_Raw(int16_t *gx, int16_t *gy, int16_t *gz)
{
    uint8_t buf[6] = {0};
    I2Cx_ReadBuff(MPU6050_I2C, MPU_ADDR, MPU_GYRO_XOUTH_REG, buf, 6);

    *gx = ((int16_t)buf[0] << 8) | buf[1];
    *gy = ((int16_t)buf[2] << 8) | buf[3];
    *gz = ((int16_t)buf[4] << 8) | buf[5];
}

/**
 * @brief 获取加速度计的加速度值(原始值)
 *
 * @param ax x轴的加速度
 * @param ay y轴的加速度
 * @param az z轴的加速度
 * @return uint8_t 
 */
uint8_t MPU_GetAccel_Raw(int16_t *ax,int16_t *ay,int16_t *az)
{
    uint8_t buf[6] = {0};
    I2Cx_ReadBuff(MPU6050_I2C, MPU_ADDR, MPU_ACCEL_XOUTH_REG, buf, 6);

    *ax = ((int16_t)buf[0] << 8) | buf[1];
    *ay = ((int16_t)buf[2] << 8) | buf[3];
    *az = ((int16_t)buf[4] << 8) | buf[5];
}

/**
 * @brief 获取陀螺仪的角速度值（单位°/s），并减去静态偏移量
 *
 * @param gx x轴的角速度
 * @param gy y轴的角速度
 * @param gz z轴的角速度
 * @param unit 指定角速度的单位：
 *      - MPU_Deg_S -> °/s
 *      - MPU_Rad_S -> rad/s
 * @return uint8_t
 */
uint8_t MPU_GetGyro(float_t *gx, float_t *gy, float_t *gz, uint8_t unit)
{
    uint8_t buf[6] = {0};
    int16_t x = 0, y = 0 ,z = 0;

    /* 读取原始值 */
    I2Cx_ReadBuff(MPU6050_I2C, MPU_ADDR, MPU_GYRO_XOUTH_REG, buf, 6);
    x = ((int16_t)buf[0] << 8) | buf[1];
    y = ((int16_t)buf[2] << 8) | buf[3];
    z = ((int16_t)buf[4] << 8) | buf[5];

    /* 减去初始校准值 */
    x -= MpuGyroOffset[0];
    y -= MpuGyroOffset[1];
    z -= MpuGyroOffset[2];

    /* 单位转换 */
    if (unit == MPU_Deg_S) {
        /* °/s */
        *gx = (float_t)x * Gyro_Gain_Deg_S;
        *gy = (float_t)y * Gyro_Gain_Deg_S;
        *gz = (float_t)z * Gyro_Gain_Deg_S;
    } else if (MPU_Rad_S) {
        /* rad/s */
        *gx = (float_t)x * Gyro_Gain_Rad_S;
        *gy = (float_t)y * Gyro_Gain_Rad_S;
        *gz = (float_t)z * Gyro_Gain_Rad_S;        
    }
}

/**
 * @brief 获取加速度计的加速度值（单位m/s2），并减去静态偏移量
 *
 * @param ax x轴的加速度
 * @param ay y轴的加速度
 * @param az z轴的加速度
 * @return uint8_t 
 */
uint8_t MPU_GetAccel(float_t *ax, float_t *ay, float_t *az)
{
    uint8_t buf[6] = {0};
    int16_t x = 0, y = 0 ,z = 0;

    /* 读取原始值 */
    I2Cx_ReadBuff(MPU6050_I2C, MPU_ADDR, MPU_ACCEL_XOUTH_REG, buf, 6);
    x = ((int16_t)buf[0] << 8) | buf[1];
    y = ((int16_t)buf[2] << 8) | buf[3];
    z = ((int16_t)buf[4] << 8) | buf[5];

    /* 减去初始校准值 */
    x -= MpuAccOffset[0];
    y -= MpuAccOffset[1];
    z -= MpuAccOffset[2];

    /* 转换单位为：m/s2 */
    *ax = (float_t)x * Acc_Gain;
    *ay = (float_t)y * Acc_Gain;
    *az = (float_t)z * Acc_Gain;
}

/**
 * @brief 获取MPU6050的所有传感器数据（温度、加速度、角速度），并减去静态偏移量
 * 
 * @param MPU6050 指向MPU6050_t类型的指针，用于保存传感器数据
 * @return uint8_t 
 */
uint8_t MPU_GetData(MPU6050_t* MPU6050)
{
    uint8_t buf[6] = {0};
    int16_t x = 0, y = 0 ,z = 0;
    int16_t raw = 0;
    float_t temp = 0;

    /* 读取陀螺仪原始值 */
    I2Cx_ReadBuff(MPU6050_I2C, MPU_ADDR, MPU_GYRO_XOUTH_REG, buf, 6);
    x = ((int16_t)buf[0] << 8) | buf[1];
    y = ((int16_t)buf[2] << 8) | buf[3];
    z = ((int16_t)buf[4] << 8) | buf[5];

    // printf("%d,%d,%d\n", x, y, z);
    /* 减去初始校准值 */
    x -= MpuGyroOffset[0];
    y -= MpuGyroOffset[1];
    z -= MpuGyroOffset[2];

    /* 角速度转换单位为：rad/s */
    MPU6050->gx = (float_t)x * Gyro_Gain_Rad_S;
    MPU6050->gy = (float_t)y * Gyro_Gain_Rad_S;
    MPU6050->gz = (float_t)z * Gyro_Gain_Rad_S;        

    /* 读取原始值 */
    I2Cx_ReadBuff(MPU6050_I2C, MPU_ADDR, MPU_ACCEL_XOUTH_REG, buf, 6);
    x = ((int16_t)buf[0] << 8) | buf[1];
    y = ((int16_t)buf[2] << 8) | buf[3];
    z = ((int16_t)buf[4] << 8) | buf[5];
    // printf("%d,%d,%d\n", x, y, z);

    /* 减去初始校准值 */
    // x -= MpuAccOffset[0];
    // y -= MpuAccOffset[1];
    // z -= MpuAccOffset[2];
    // printf("%d,%d,%d\n", x, y, z);

    /* 加速度转换单位为：m/s2 */
    MPU6050->ax = (float_t)x * Acc_Gain;
    MPU6050->ay = (float_t)y * Acc_Gain;
    MPU6050->az = (float_t)z * Acc_Gain;

    /* 读取温度 */
    I2Cx_ReadBuff(MPU6050_I2C, MPU_ADDR, MPU_TEMP_OUTH_REG, buf, 2);

    raw = ((int16_t)buf[0] << 8) | buf[1];          // 合并高位和低位
    temp = ((float_t)raw / 340.0f + 36.53f ) * 100; // 转换为摄氏度
    MPU6050->temp = temp / 100.0f;
}

/**
 * @brief 校准 MPU6050，采样一定数量的值作为传感器校准值
 * 
 * @param samples 采样数量
 * @param delay_ms 采样间隔
 */
void MpuGetOffset(uint16_t samples, uint16_t delay_ms)
{
    int32_t acc_sum[3]  = {0, 0, 0};
    int32_t gyro_sum[3] = {0, 0, 0};
    int16_t ax_raw, ay_raw, az_raw;
    int16_t gx_raw, gy_raw, gz_raw;

    /* 预热传感器 */
    for (int i=0; i<50; i++) {
        MPU_GetAccel_Raw(&ax_raw, &ay_raw, &az_raw);
        MPU_GetGyro_Raw(&gx_raw, &gy_raw, &gz_raw);
        Delay_ms(3);
    }

    /* 采集静态样本 */
    for (int i=0; i<samples; i++) {
        MPU_GetAccel_Raw(&ax_raw, &ay_raw, &az_raw);
        MPU_GetGyro_Raw(&gx_raw, &gy_raw, &gz_raw);

        /* 累计求和 */
        acc_sum[0] += ax_raw;
        acc_sum[1] += ay_raw;
        acc_sum[2] += az_raw;
        gyro_sum[0] += gx_raw;
        gyro_sum[1] += gy_raw;
        gyro_sum[2] += gz_raw;

        Delay_ms(delay_ms);
    }

    /* 取静态样本的平均值为偏移误差 */
    MpuAccOffset[0] = acc_sum[0] / samples;
    MpuAccOffset[1] = acc_sum[1] / samples;
    MpuAccOffset[2] = acc_sum[2] / samples - Acc_LSBSens;// Z轴偏移量（减去1g对应的理论值）
    MpuGyroOffset[0] = gyro_sum[0] / samples;
    MpuGyroOffset[1] = gyro_sum[1] / samples;
    MpuGyroOffset[2] = gyro_sum[2] / samples;
    
}
